Efficient Construction of Optimal and Consistent Ladar Maps Using Pose Network Topology and Nonlin- ear Programming

Many forms of mobile robot guidance map can be visualized as a network of nodes (images) connected by edges (poses). We show how explicit reasoning about the loop topology of this pose network can generate a cycle basis for the network in linear time. This basis can then be used to form the independent constraint equations in a natural constrained optimization formulation of the mapping problem. In many practical cases, the pose network has sparse structure, and the associated equations can then be solved in linear time. This approach can be used to construct optimal, consistent maps on very large scales in very limited computation. While the technique is applicable to mapbuilding in general, and even optimization in general, it is illustrated here for batch processing of 2D ladar scans into a mobile robot guidance map.